byte ignore = 0;
int xcommand = 0;

// Motordriver PINs
int rdir = 3; // PIN for right wheel direction
boolean rdir_v = false; // Value for ridr
int ldir = 4; // PIN for left wheel direction
boolean ldir_v = false; // Value for ldir
int rspeedpin = 10; // PIN for right wheel speed
int lspeedpin = 11; // PIN for left wheel speed
// Motorstates
boolean motorrun = false; // status for the motor - running or not
int initialspeed = 100; // Value for the initial speed for rspeed and lspeed
int rcurr_speed = 0; // Value for the current speed of the right wheel
int lcurr_speed = 0; // Value for the current speed of the left wheel

// Distance Pins
int front_pin = 7; // PIN for the front-sensor
int back_pin = 8; // PIN for the back-sensor
int right_pin = 9; // PIN for the right-sensor
int left_pin = 5; // PIN for the left-sensor
// Distance States
int front_dist = 0; // Value for the current distance - front
int back_dist = 0; // Value for the current distance - back
int right_dist = 0; // Value for the current distance - right
int left_dist = 0; // Value for the current distance - left

int acc_speed = 40; // Acceleration Speed

// Build Navigation-Array
boolean navigation[10][10] = {
  {false,false,false,false,false,false,false,false,false,false,},
  {false,false,false,false,false,false,false,false,false,false,},
  {false,false,false,false,false,false,false,false,false,false,},
  {false,false,false,false,false,false,false,false,false,false,},
  {false,false,false,false,false,false,false,false,false,false,},
  {false,false,false,false,false,false,false,false,false,false,},
  {false,false,false,false,false,false,false,false,false,false,},
  {false,false,false,false,false,false,false,false,false,false,},
  {false,false,false,false,false,false,false,false,false,false,},
  {false,false,false,false,false,false,false,false,false,false,}
};



// DEBUG Options
int debug_pin = 13;
int debug_pin_01_state = LOW;
long blink_temp_01 = 0;
long blink_interval_01 = 1000;

int debug2_pin = 2;
int debug_pin_02_state = LOW;
long blink_temp_02 = 0;
long blink_interval_02 = 100;

void setup()
{
  Serial.begin(9600);
  pinMode(rdir, OUTPUT);
  pinMode(ldir, OUTPUT);
  pinMode(rspeedpin, OUTPUT);
  pinMode(lspeedpin, OUTPUT);
  pinMode(front_pin, INPUT);
  pinMode(back_pin, INPUT);
  pinMode(right_pin, INPUT);
  pinMode(left_pin, INPUT);
  pinMode(debug_pin, OUTPUT);
  pinMode(debug2_pin, OUTPUT);
  debug2();
}

void loop(){
  
  //distance();
  
  //volt_to_cm();
  
  if (Serial.available() >= 17){
    xread();
  }
  
  debug_led01();
  debug2_timer();
  
  
}
  

void xread(){
  if (Serial.read() == 0x7E){
    ignore = Serial.read(); // 0x00
    ignore = Serial.read(); // 0x0D
    ignore = Serial.read(); // 0x90
    ignore = Serial.read(); // 0x00
    ignore = Serial.read(); // 0x13
    ignore = Serial.read(); // 0xA2
    ignore = Serial.read(); // 0x00
    ignore = Serial.read(); // 0x40
    ignore = Serial.read(); // 0x76
    ignore = Serial.read(); // 0x94
    ignore = Serial.read(); // 0x49
    ignore = Serial.read(); // 0x00
    ignore = Serial.read(); // 0x00
    ignore = Serial.read(); // 0x01
    xcommand = Serial.read(); // 0x0B         
    ignore = Serial.read(); // 0x1B - Checksum
  }
  control();
}


void control(){
  switch (xcommand) {
    case 0x01: //forward
      rdir_v = true;
      ldir_v = true;
      lcurr_speed = initialspeed;
      rcurr_speed = initialspeed;
      motorcontrol();
      debug2();
      break;
    
    case 0x02: //rewind
      rdir_v = false;
      ldir_v = false;
      lcurr_speed = initialspeed;
      rcurr_speed = initialspeed;
      motorcontrol();
      debug2();
      break;
    
    case 0x03: //right
      if (ldir_v == true){
        if (lcurr_speed - acc_speed > 0){
          lcurr_speed = lcurr_speed - acc_speed;
        }
        else{
          lcurr_speed = 0;
        }
      }
      else{
        if (lcurr_speed + acc_speed < 254){
          lcurr_speed = lcurr_speed + acc_speed;
        }
        else{
          lcurr_speed = 254;
        }
      }
      motorcontrol();
      debug2();
      break;
    
    case 0x04: //sharp right
      lcurr_speed = 0;
      motorcontrol();
      debug2();
      break;

    case 0x05: // left
      if (rdir_v == true){
        if (rcurr_speed - acc_speed > 0){
          rcurr_speed = rcurr_speed - acc_speed;
        }
        else{
          rcurr_speed = 0;
        }
      }
      else{
        if (rcurr_speed + acc_speed < 254){
          rcurr_speed = rcurr_speed + acc_speed;
        }
        else{
          rcurr_speed = 254;
        }
      }
          
      motorcontrol();
      debug2();
      break;
      
    case 0x06: // sharp left
      rcurr_speed = 0;
      motorcontrol();
      debug2();
      break;
    
    case 0x07: // slower both wheels
      if (ldir_v == true){
        if (lcurr_speed + acc_speed < 255){
          lcurr_speed = lcurr_speed + acc_speed;
        }
        else{
          lcurr_speed = 255;
        }
      }
      else{
        if (lcurr_speed - acc_speed > 0){
          lcurr_speed = lcurr_speed - acc_speed;
        }
        else{
          lcurr_speed = 0;
        }        
      }
      if (rdir_v == true){
        if (rcurr_speed + acc_speed < 255){
          rcurr_speed = rcurr_speed + acc_speed;
        }
        else rcurr_speed = 255;
      }
      else{
        if (rcurr_speed - acc_speed > 0){
          rcurr_speed = rcurr_speed - acc_speed;
        }
        else{
          rcurr_speed = 0;
        }
      }
      lcurr_speed = lcurr_speed + acc_speed;
      rcurr_speed = rcurr_speed + acc_speed;
      motorcontrol();
      debug2();
      break;
    
    case 0x10: // faster both wheels
      if (ldir_v == true){
        if (lcurr_speed - acc_speed > 0){
          lcurr_speed = lcurr_speed - acc_speed;
        }
        else{
          lcurr_speed = 0;
        }
      }
      else{
        if (lcurr_speed + acc_speed < 255){
          lcurr_speed = lcurr_speed + acc_speed;
        }
        else{
          lcurr_speed = 255;
        }
      }
      
      if (rdir_v == true){
          if (rcurr_speed - acc_speed > 0){
            rcurr_speed = rcurr_speed - acc_speed;
          }
          else{
            rcurr_speed = 0;
          }
        }
        else{
          if (rcurr_speed + acc_speed < 255){
            rcurr_speed = rcurr_speed + acc_speed;
          }
          else{
            rcurr_speed = 255;
          }
        }
          
      motorcontrol();
      debug2();
      break;

    case 0x11: // STOP
      rcurr_speed = 0;
      lcurr_speed = 0;
      rdir_v = false;
      ldir_v = false;
      motorcontrol();
      debug2();
      break;
  }
}
  
void motorcontrol(){
  
  if (rdir_v == true){
    digitalWrite(rdir, HIGH);
  }
  else{
    digitalWrite(rdir, LOW);
  }
    
  if (ldir_v == true){
    digitalWrite(ldir, HIGH);
  }
  else{
    digitalWrite(ldir, LOW);
  }
  
    analogWrite(rspeedpin, rcurr_speed);

    analogWrite(lspeedpin, lcurr_speed);
}

void distance(){
  front_dist = analogRead(front_pin);
  back_dist = analogRead(back_pin);
  right_dist = analogRead(right_pin);
  left_dist = analogRead(left_pin);
}

void volt_to_cm(){
  
  long A = 0.0082712905;
  long B = 939.57652;
  long C =  -3.397869;
  long D = 17.339222;
  int V = (analogRead(front_pin)*5)/1024;
  float distance = (A + B * V) / (1 + C * V + D * V * V);
}

void debug_led01(){
  
  if (blink_temp_01 != 0){
    if (millis() - blink_temp_01 > blink_interval_01){
      blink_temp_01 = millis();
      if (debug_pin_01_state == LOW){
        debug_pin_01_state = HIGH;
      }
      else{
        debug_pin_01_state = LOW;
      }
    }
    digitalWrite(debug_pin, debug_pin_01_state);
  }
  else{
    blink_temp_01 = millis();
    digitalWrite(debug_pin, debug_pin_01_state);
  }  
}

void debug2(){
  blink_temp_02 = millis();
  debug_pin_02_state = HIGH;
  digitalWrite(debug2_pin, debug_pin_02_state);
}
  
void debug2_timer(){
  if (millis() - blink_temp_02 > blink_interval_02){
      debug_pin_02_state = LOW;
    }
  digitalWrite(debug2_pin, debug_pin_02_state);
}
